#include "utility.h"
#include <QDebug>


Utility::Utility()
{
}

cv::Mat Utility::convertToBinary(cv::Mat inputImage){
    // Convert to binary Image (0 and 255)
    cv::Mat grayImage;
    cv::cvtColor(inputImage,grayImage,CV_RGB2GRAY);
    cv::Mat binaryImage = grayImage > THRESHOLD;

    // Morphological filter
    // Init struct element
    cv::Mat structElement = getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(2*STRUCT_ELEMENT_SIZE+1,2*STRUCT_ELEMENT_SIZE+1));
    // Use closing operator
    cv::dilate( binaryImage, binaryImage, structElement);
    cv::erode( binaryImage, binaryImage, structElement);
    // Use opening operator
    cv::erode( binaryImage, binaryImage, structElement);
    cv::dilate( binaryImage, binaryImage, structElement);

    return binaryImage;

}

std::vector<cv::Point> Utility::findContour(cv::Mat inputImage){
    cv::Mat cannyImage;
    std::vector<std::vector<cv::Point> > contours;
    std::vector<cv::Vec4i> hierarchy;
    std::vector<cv::Point> result;
    cv::Canny( inputImage, cannyImage, THRESHOLD - 30, THRESHOLD+30, 3);
    cv::findContours( cannyImage, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );


    for( int i = 0; i< contours.size(); i++ ){
            std::vector<cv::Point> contour;
            contour = contours[i];
            for(int j = 0; j < contour.size(); j++){
                if (std::find(result.begin(), result.end(),contour[j]) == result.end()){
                    result.push_back(contour[j]);
                }
            }
    }
    return result;
}

double Utility::calEuclidianDistance(cv::Point refPoint, cv::Point chordPoint1, cv::Point chordPoint2){
    double u = refPoint.x*(chordPoint1.y - chordPoint2.y ) + refPoint.y*(chordPoint2.x - chordPoint1.x) + chordPoint2.y*chordPoint1.x - chordPoint1.y*chordPoint2.x;
    double v = sqrt((chordPoint1.y - chordPoint2.y)*(chordPoint1.y - chordPoint2.y) + (chordPoint1.x - chordPoint2.x)*(chordPoint1.x - chordPoint2.x));
    return abs((double)u/(double)v);


}

std::vector<double> Utility::obtainingLCS(std::vector<cv::Point> contour, int windowSize){
    std::vector<double> result;
    double euclidianDistance = 0;
    for (int i = (windowSize -1)/2; i < contour.size() - ((windowSize -1)/2); ++i) {
        cv::Point refPoint = contour[i];
        cv::Point chordPoint1 = contour[i - (windowSize -1)/2];
        cv::Point chordPoint2 = contour[i + (windowSize -1)/2];
        euclidianDistance = Utility::calEuclidianDistance(refPoint, chordPoint1, chordPoint2);
        result.push_back(euclidianDistance);
    }
    return result;
}

std::vector<double> Utility::linearAligment(std::vector<double> originalVector){
    std::vector<double> result = Utility::normalizedDuration(originalVector, Utility::LINEAR_DURATION_SIZE);
    result = Utility::normalizedAmplitude(result);
    return result;
}

std::vector<double> Utility::normalizedAmplitude(std::vector<double> originalVector){
    int sum = 0;
    for (int index = 0; index < originalVector.size(); ++index){
        sum += originalVector[index];
    }
    double mean = (double)sum/originalVector.size();
    double sumStandardDeviation = 0;
    for (int index = 0; index < originalVector.size(); ++index){
        sumStandardDeviation += (originalVector[index] - mean)*(originalVector[index] - mean);
    }
    double standardDeviation = sqrt(sumStandardDeviation/originalVector.size());

    std::vector<double> result;
    for (int index = 0; index < originalVector.size(); ++index){
        result.push_back(originalVector[index]/standardDeviation);
    }
    return result;

}

std::vector<double> Utility::normalizedDuration(std::vector<double> originalVector, int newSize){
    double oldIndex;
    int lowBound;
    int oldSize = originalVector.size();
    int newValue = 0;
    std::vector<double> newVector;
    for(int index = 0; index < newSize; index++){
        oldIndex = index*((double)(oldSize-1)/(newSize-1));
        lowBound = (int)oldIndex;
        newValue = originalVector[lowBound]*(lowBound+1 - oldIndex) + originalVector[lowBound+1] *(oldIndex - lowBound);
        newVector.push_back(newValue);
    }
    return newVector;
}

double Utility::findLinearBestMatching(std::vector<double> refVecto, std::vector<double> sampleVector){
    int size = sampleVector.size();
    double AsampleVector[size];
    double ArefVecto[size];
    for(int i=0;i<size;i++)
    {
        AsampleVector[i] = sampleVector[i];
        ArefVecto[i] = refVecto[i];
    }

    int refIndex = 0;
    int sampleIndex = 0;
    double Dmin = 99999999;
    double D ;
    for (int circulartShift = 0; circulartShift < size; ++circulartShift) {
        D = 0;
        for (refIndex = 0;  refIndex< size; ++refIndex) {
            sampleIndex = (refIndex + circulartShift)%size;
            //qDebug()<<"dasd"<<refVecto[refIndex]<<sampleVector[sampleIndex]<<refVecto[refIndex] - sampleVector[sampleIndex];
            D += qAbs(ArefVecto[refIndex] - AsampleVector[sampleIndex]);
        }
        if(D < Dmin){
            Dmin = D;
        }
    }
    return Dmin;
}



double Utility::findNonlinearBestMatching(std::vector<double> refVecto, std::vector<double> sampleVector){
    double  **differentMeasure;
    differentMeasure = new double*[MAX_VECTOR_SIZE];
    for(int z=0;z<300;z++)
        differentMeasure[z] = new double[MAX_VECTOR_SIZE];
    int size = refVecto.size();
    double AAsampleVector[size];
    double AsampleVector[size];
    double ArefVecto[size];
    for(int i=0;i<size;i++)
    {
        AAsampleVector[i] = sampleVector[i];
        ArefVecto[i] = refVecto[i];
    }

    double Dmin = 999999;
    for (int circulartShift = 0; circulartShift < size; ++circulartShift) {

        for(int i=0;i<size;i++)
        {
            int sampleIndex = (i + circulartShift)%size;
            AsampleVector[sampleIndex] = AAsampleVector[sampleIndex];
        }
        differentMeasure[0][0] = qAbs(ArefVecto[0] - AsampleVector[0]);
        for (int i = 1; i < size; ++i) {
            differentMeasure[0][i] = qAbs(ArefVecto[0] - AsampleVector[i]) + differentMeasure[0][i-1];
            differentMeasure[i][0] = qAbs(ArefVecto[i] - AsampleVector[0]) + differentMeasure[i-1][0];
        }

        for (int i = 1; i < size; ++i) {
            for (int j = 1; j < size; ++j) {
                differentMeasure[i][j] = qAbs(ArefVecto[i] - AsampleVector[j]) + qMin(differentMeasure[i][j-1], qMin(differentMeasure[i-1][j], differentMeasure[i-1][j-1]));
            }
        }

        Dmin = qMin(Dmin,differentMeasure[size-1][size-1]);
    }
    for(int z=0;z<MAX_VECTOR_SIZE;z++)
        delete differentMeasure[z];
    delete differentMeasure;
    return Dmin;
    //return 0;
}

//double Utility::findNonlinearBestMatching(std::vector<double> refVecto, std::vector<double> sampleVector){
//    double  **differentMeasure;
//    differentMeasure = new double*[MAX_VECTOR_SIZE];
//    for(int z=0;z<300;z++)
//        differentMeasure[z] = new double[MAX_VECTOR_SIZE];
//    int size = refVecto.size();
//    double AsampleVector[size];
//    double ArefVecto[size];
//    for(int i=0;i<size;i++)
//    {
//        AsampleVector[i] = sampleVector[i];
//        ArefVecto[i] = refVecto[i];
//    }


//    for (int i = 0; i < size; ++i) {
//        differentMeasure[0][i] = qAbs(ArefVecto[0] - AsampleVector[i]);
//        differentMeasure[i][0] = qAbs(ArefVecto[i] - AsampleVector[0]);
//    }

//    for (int i = 1; i < size; ++i) {
//        for (int j = 1; j < size; ++j) {
//            differentMeasure[i][j] = qAbs(ArefVecto[i] - AsampleVector[j]) + qMin(differentMeasure[i][j-1], qMin(differentMeasure[i-1][j], differentMeasure[i-1][j-1]));
//        }
//    }

//    double result = differentMeasure[size-1][size-1];
//    for(int z=0;z<MAX_VECTOR_SIZE;z++)
//        delete differentMeasure[z];
//    delete differentMeasure;
//    return result;
//    //return 0;
//}
double Utility::findMin(double number1, double number2, double number3){
    double min;
    if(number1 < number2){
        min = number1;
    }else{
        min = number2;
    }

    if(min < number3){
        return min;
    }else{
        return number3;
    }
}
